#!/usr/bin/env python3
# encoding: utf-8
# @Author: Zecous
# @Date: 2025/07/18
import cv2
import numpy as np
import rospy
import signal
import time
import math
from sensor_msgs.msg import Image
from std_msgs.msg import String, Bool
from std_srvs.srv import Trigger, TriggerResponse
from cv_bridge import CvBridge
from ainex_sdk import misc, common
from ainex_example.color_common import Common

class StairsTracker(Common):
    def __init__(self, name):
        # 初始化头部位置
        self.head_pan_init = 500   # 左右舵机的初始值
        self.head_tilt_look_down = 280  # 低头位置
        
        # 初始化ROS节点
        rospy.init_node(name)
        self.name = name
        self.running = True
        self.active = False  # 默认不激活，等待外部启动
        
        super().__init__(name, self.head_pan_init, self.head_tilt_look_down)
        
        # 设置退出处理函数
        signal.signal(signal.SIGINT, self.shutdown)
        
        # Initialize CV Bridge
        self.bridge = CvBridge()
        
        # State variables - 简化控制流程
        self.current_image = None
        self.current_state = "aligning_climb"  # aligning_climb -> climb_ready -> descend_ready -> completed
        self.climb_executed = False
        self.descend_executed = False
        self.task_completed = False
        
        # 从ROS参数服务器加载配置
        self.load_config()
        
        # 初始化运动参数
        self.init_movement_params()
        
        # 图像参数
        self.image_width = rospy.get_param('~image_width', 640)
        self.image_height = rospy.get_param('~image_height', 480)
        self.center_x = self.image_width // 2
        
        # 初始化ROI检测区域参数（默认值）
        self.roi_y_ratio = 0.6  # 检测区域高度比例（0.6表示下方60%）
        self.roi_y_start = int(self.image_height * (1 - self.roi_y_ratio))  # 从(1-ratio)高度开始
        self.roi_y_end = self.image_height  # 到画面底部
        
        # 从配置加载位置判断参数
        self.load_position_params()
        
        # ROS通信接口
        self.setup_ros_interface()

        # 显示窗口控制
        self.update_display_config()
        
        # Subscribe to camera topic
        self.image_subscriber = rospy.Subscriber('/camera/image_raw', Image, self.image_callback)
        
        # 初始化机器人动作
        self.motion_manager.run_action('walk_ready')
        
        # 设置低头位置
        self.set_servo(self.head_pan_init, self.head_tilt_look_down)
        time.sleep(2)  # 等待舵机到位
        
        common.loginfo('Stairs tracker initialized with simplified control')
        print("Stairs task ready, waiting for activation...")

    def update_display_config(self):
        """从参数服务器更新显示配置"""
        display_ns = '/system/display'
        main_flag = rospy.get_param(f'{display_ns}/show_stairs_window', None)
        if main_flag is None:
            main_flag = rospy.get_param(f'{display_ns}/show_main_window', True)
        self.display_main_window = bool(main_flag)

        mask_flag = rospy.get_param(f'{display_ns}/show_detection_masks', True)
        self.display_mask_window = self.display_main_window and bool(mask_flag)

    @staticmethod
    def _safe_destroy_window(window_name):
        try:
            cv2.destroyWindow(window_name)
        except cv2.error:
            pass
        except Exception:
            pass

    def setup_ros_interface(self):
        """设置ROS通信接口"""
        # 状态发布器
        self.status_pub = rospy.Publisher('/stairs/status', String, queue_size=1)
        self.active_pub = rospy.Publisher('/stairs/active', Bool, queue_size=1)
        self.progress_pub = rospy.Publisher('/stairs/progress', String, queue_size=1)
        
        # 控制服务
        self.start_service = rospy.Service('/stairs/start', Trigger, self.start_service_callback)
        self.stop_service = rospy.Service('/stairs/stop', Trigger, self.stop_service_callback)
        self.reset_service = rospy.Service('/stairs/reset', Trigger, self.reset_service_callback)
        
        # 控制命令订阅
        self.control_sub = rospy.Subscriber('/stairs/control', String, self.control_callback)

    def start_service_callback(self, req):
        """启动服务回调"""
        self.active = True
        self.running = True
        self.task_completed = False
        self.climb_executed = False
        self.descend_executed = False
        self.current_state = "aligning_climb"
        self.publish_status("started")
        return TriggerResponse(success=True, message="Stairs task started")

    def stop_service_callback(self, req):
        """停止服务回调"""
        self.active = False
        self.gait_manager.stop()
        self.publish_status("stopped")
        return TriggerResponse(success=True, message="Stairs task stopped")

    def reset_service_callback(self, req):
        """重置服务回调"""
        self.active = False
        self.gait_manager.stop()
        self.current_state = "aligning_climb"
        self.climb_executed = False
        self.descend_executed = False
        self.task_completed = False
        self.publish_status("reset")
        return TriggerResponse(success=True, message="Stairs task reset")

    def control_callback(self, msg):
        """控制命令回调"""
        command = msg.data.lower()
        if command == "start":
            self.active = True
            self.running = True
            self.task_completed = False
        elif command == "stop":
            self.active = False
            self.gait_manager.stop()
        elif command == "reset":
            self.active = False
            self.gait_manager.stop()
            self.current_state = "aligning_climb"
            self.climb_executed = False
            self.descend_executed = False
            self.task_completed = False

    def publish_status(self, status):
        """发布状态"""
        msg = String()
        msg.data = status
        self.status_pub.publish(msg)
        
        active_msg = Bool()
        active_msg.data = self.active
        self.active_pub.publish(active_msg)
        
        progress_msg = String()
        progress_msg.data = f"State: {self.current_state}, Climb: {self.climb_executed}, Descend: {self.descend_executed}, Completed: {self.task_completed}"
        self.progress_pub.publish(progress_msg)

    def load_config(self):
        """从ROS参数服务器加载配置"""
        max_retries = 3
        
        for attempt in range(max_retries):
            try:
                # 从参数服务器获取颜色配置
                try:
                    green_config = rospy.get_param('/image_processing/colors/green', None)
                    if green_config and 'lower' in green_config and 'upper' in green_config:
                        self.green_lower = np.array(green_config['lower'])
                        self.green_upper = np.array(green_config['upper'])
                        print(f"从参数服务器加载绿色配置: {self.green_lower} ~ {self.green_upper}")
                        break
                    else:
                        raise Exception("绿色配置不完整")
                except Exception as e:
                    print(f"获取绿色配置失败: {e}")
                    if attempt < max_retries - 1:
                        rospy.sleep(1.0)
                        continue
                    else:
                        # 使用默认配置
                        self.green_lower = np.array([136, 0, 155])
                        self.green_upper = np.array([255, 255, 255])
                        print("使用默认绿色配置")
                        break
                        
            except Exception as e:
                print(f"配置加载失败: {e}")
                if attempt == max_retries - 1:
                    # 使用默认配置
                    self.green_lower = np.array([136, 0, 155])
                    self.green_upper = np.array([255, 255, 255])
        
        # 从参数服务器获取舵机配置
        try:
            servo_config = rospy.get_param('/servo', {})
            self.head_pan_init = servo_config.get('head_pan_init', 500)
            self.head_tilt_look_down = servo_config.get('head_tilt_look_down', 280)
        except Exception as e:
            print(f"舵机配置加载失败: {e}")
            self.head_pan_init = 500
            self.head_tilt_look_down = 280

    def load_position_params(self):
        """从配置加载位置判断参数"""
        try:
            # 从参数服务器获取台阶任务配置
            stairs_config = rospy.get_param('/tasks/stairs', {})
            position_ratios = stairs_config.get('position_ratios', {})
            
            # 加载位置比例参数
            climb_target_y_ratio = position_ratios.get('climb_target_y_ratio', 0.2)
            descend_target_y_ratio = position_ratios.get('descend_target_y_ratio', 0.6)
            self.climb_target_y = int(self.image_height * climb_target_y_ratio)
            self.descend_target_y = int(self.image_height * descend_target_y_ratio)
            
            # 加载ROI检测区域参数
            if 'roi_y_ratio' in position_ratios:
                self.roi_y_ratio = position_ratios.get('roi_y_ratio', 0.6)
                self.roi_y_start = int(self.image_height * (1 - self.roi_y_ratio))
                self.roi_y_end = self.image_height  # 到画面底部
            
            # 从参数服务器获取运动参数
            movement_config = stairs_config.get('movement', {})
            self.descend_forward_steps = movement_config.get('descend_forward_steps', 3)
            
            # print(f"位置参数加载成功 - 上台阶目标Y位置: {self.climb_target_y} (比例: {climb_target_y_ratio})")
            # print(f"位置参数加载成功 - 下台阶目标Y位置: {self.descend_target_y} (比例: {descend_target_y_ratio})")
            # print(f"ROI检测区域: 下方 {self.roi_y_ratio*100:.0f}% (Y: {self.roi_y_start}-{self.roi_y_end})")
            # print(f"运动参数 - 下台阶前前进步数: {self.descend_forward_steps}")
            
        except Exception as e:
            print(f"位置参数加载失败，使用默认值: {e}")
            # 使用默认配置
            self.climb_target_y = int(self.image_height * 0.2)
            self.descend_target_y = int(self.image_height * 0.6)
            self.descend_forward_steps = 3

    def init_movement_params(self):
        """初始化运动参数"""
        self.calib_config = common.get_yaml_data('/home/ubuntu/ros_ws/src/ainex_example/config/calib.yaml')
        
        # 获取步态参数
        self.walking_param = self.gait_manager.get_gait_param()
        self.walking_param['angle_move_amplitude'] = 2.5   
        self.walking_param['step_height'] = 0.02    # 步高
        self.walking_param['pelvis_offset'] = 2      # 盆骨偏移
        self.walking_param['step_fb_ratio'] = 0.028
        self.walking_param['init_roll_offset'] = -6.0
        self.walking_param['hip_pitch_offset'] = 8.5  # 髋关节偏移
        self.walking_param['init_y_offset'] = -0.005    # 初始y偏移
        
        # 运动参数设置
        self.dsp_param = [350, 0.2, 0.025]  # DSP参数
        self.arm_swap = 30                   # 摆臂幅度
        self.step_distance = 0.009         # 前进步长

    def image_callback(self, msg):
        """ROS图像回调函数"""
        try:
            self.current_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        except Exception as e:
            rospy.logerr(f"Failed to convert image: {e}")

    def detect_green_line(self, image):
        """检测绿色线条"""
        if image is None:
            return None, None
        
        # 转换为LAB色彩空间
        lab = cv2.cvtColor(image, cv2.COLOR_BGR2LAB)
        
        # 创建绿色掩码
        mask = cv2.inRange(lab, self.green_lower, self.green_upper)
        
        # 创建ROI掩码 - 仅检测画面下方指定区域
        roi_mask = np.zeros_like(mask)
        roi_mask[self.roi_y_start:self.roi_y_end, :] = 255
        
        # 应用ROI限制
        mask = cv2.bitwise_and(mask, roi_mask)
        
        # 形态学操作去除噪声
        kernel = np.ones((5, 5), np.uint8)
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
        
        # 使用霍夫直线变换检测线条
        lines = cv2.HoughLinesP(mask, 1, np.pi/180, threshold=50, minLineLength=50, maxLineGap=20)
        
        if lines is not None and len(lines) > 0:
            # 找到最长的线条
            longest_line = None
            max_length = 0
            
            for line in lines:
                x1, y1, x2, y2 = line[0]
                length = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
                if length > max_length:
                    max_length = length
                    longest_line = line[0]
            
            if longest_line is not None:
                x1, y1, x2, y2 = longest_line
                
                # 计算线条中心点和角度
                center_x = (x1 + x2) / 2
                center_y = (y1 + y2) / 2
                
                # 计算角度（相对于水平线）
                if x2 != x1:
                    angle = math.atan2(y2 - y1, x2 - x1) * 180 / math.pi
                else:
                    angle = 90  # 垂直线
                
                # 标准化角度到-90到90度
                while angle > 90:
                    angle -= 180
                while angle < -90:
                    angle += 180
                
                return {
                    'line': longest_line,
                    'center': (center_x, center_y),
                    'angle': angle,
                    'length': max_length
                }, mask
        
        return None, mask

    def get_current_action_type(self):
        """获取当前动作类型"""
        if self.current_state in ["aligning_climb", "climb_ready"]:
            return "climb"
        elif self.current_state in ["aligning_descend", "descend_ready"]:
            return "descend"
        return None

    def control_robot_movement(self, green_line_info):
        """控制机器人移动 - 上楼对齐，下楼直接执行"""
        if not self.active:  # 如果任务未激活，不执行移动
            return
        
        # 下楼阶段不依赖视觉对齐；仅在上楼对齐阶段缺失视觉时进入搜索
        if green_line_info is None and self.current_state == "aligning_climb":
            print("No green line detected, searching...")
            self.gait_manager.stop()
            self.publish_status("searching")
            return
        
        # 获取线条信息
        angle = green_line_info['angle']
        center_x, center_y = green_line_info['center']
        action_type = self.get_current_action_type()
        
        print(f"Green line: angle={angle:.1f}°, center=({center_x:.0f}, {center_y:.0f}), state={self.current_state}")
        
        if self.current_state == "aligning_climb":
            # 上台阶：边对齐边前进
            self.aligning_control(green_line_info, action_type)
        elif self.current_state == "aligning_descend":
            # 直接执行下台阶流程
            self.execute_descend_stairs()
        elif self.current_state == "climb_ready":
            # 准备执行上台阶
            self.execute_climb_stairs()
        elif self.current_state == "descend_ready":
            # 准备执行下台阶
            self.execute_descend_stairs()

    def aligning_control(self, green_line_info, action_type):
        """对齐控制 - 参考hurdles_node.py逻辑，边对齐边前进"""
        angle = green_line_info['angle']
        center_x, center_y = green_line_info['center']
        
        # 获取对应的目标位置
        target_y = self.climb_target_y if action_type == "climb" else self.descend_target_y
        
        print(f"Aligning {action_type}: angle={angle:.1f}°, center=({center_x:.0f}, {center_y:.0f}), target_y={target_y}")
        
        # 检查是否到达目标位置，直接执行动作
        if center_y >= target_y:
            print(f"Reached target position for {action_type}! Ready for action.")
            if action_type == "climb":
                self.current_state = "climb_ready"
            else:
                self.current_state = "descend_ready"
            self.publish_status(f"{action_type}_ready")
            return
        
        # 计算与图像中心的水平偏差
        deviation_x = center_x - self.center_x
        
        # 根据角度和偏差计算转向（参考hurdles_node.py逻辑）
        yaw_output = 0
        
        print(f"Deviation = {deviation_x}")
        # 基于位置偏差的左右调整
        # if abs(deviation_x) > 3:  # 偏差阈值
        #     if deviation_x > 0:  # 绿线偏右，机器人需要右转
        #         yaw_output += -(abs(deviation_x) / 12) * 0.5
        #     else:  # 绿线偏左，机器人需要左转
        #         yaw_output += abs(deviation_x / 12) * 0.5
        
        # 基于角度的方向调整
        if abs(angle) > 3:  # 角度阈值
            if angle > 0:  # 绿线右端向上，机器人需要右转对齐
                yaw_output += -(abs(angle) / 15) * 5
            else:  # 绿线左端向上，机器人需要左转对齐
                yaw_output += abs(angle) / 15 * 5
        
        # 限制转向幅度
        yaw_output = max(-8, min(8, yaw_output))
        print(f"yaw_output: {yaw_output}")
        
        try:
            # 执行移动（参考hurdles_node.py参数）
            self.gait_manager.set_step(
                self.dsp_param,      # DSP参数
                self.step_distance,  # x方向步长（前进）
                0,                   # y方向步长（不侧移）
                int(yaw_output),     # yaw角度（转向）
                self.walking_param,  # 步态参数
                self.arm_swap,       # 摆臂幅度
                0                    # 连续移动
            )
            
            print(f"Aligning {action_type}: moving forward with yaw: {yaw_output}, step_distance: {self.step_distance}")
            self.publish_status(f"aligning_{action_type}")
            
        except Exception as e:
            print(f"Error during {action_type} aligning: {e}")
            rospy.logerr(f"{action_type} aligning error: {e}")
            self.publish_status("error")



    def execute_climb_stairs(self):
        """执行上台阶动作"""
        if self.climb_executed:
            return
            
        try:
            print("Executing climb stairs action...")
            self.publish_status("executing_climb")
            
            # 停止步态管理器
            self.gait_manager.stop()
            time.sleep(0.5)
            
            # 禁用步态管理器
            self.gait_manager.disable()
            
            # 执行上台阶动作
            self.motion_manager.run_action('climb_stairs')
            
            # 等待动作完成
            # time.sleep(5)
            
            # 恢复准备状态
            self.motion_manager.run_action('walk_ready')
            
            self.climb_executed = True
            # 进入下楼流程
            self.current_state = "descend_ready"
            print("Climb stairs action completed! Proceeding to descend without alignment...")
            self.publish_status("climb_completed")
            # 执行下台阶（包含固定步数前进）
            self.execute_descend_stairs()
            common.loginfo('Climb stairs action executed successfully')
            
        except Exception as e:
            print(f"Error during climb stairs action: {e}")
            rospy.logerr(f"Climb stairs action error: {e}")
            self.publish_status("error")

    def execute_descend_stairs(self):
        """执行下台阶动作 - 先固定步数前进，再下台阶"""
        if self.descend_executed:
            return
            
        try:
            print(f"Executing descend stairs action - first moving forward {self.descend_forward_steps} steps...")
            self.publish_status("executing_descend")
            
            # 先向前走固定步数
            print(f"Moving forward {self.descend_forward_steps} steps before descending...")
            self.gait_manager.set_step(
                self.dsp_param,      # DSP参数
                self.step_distance,  # x方向步长（前进）
                0,                   # y方向步长（不侧移）
                2,                   # yaw角度（转向）
                self.walking_param,  # 步态参数
                self.arm_swap,       # 摆臂幅度
                self.descend_forward_steps    
            )
            
            # 等待前进完成
            time.sleep(2)
            
            # 停止步态管理器
            self.gait_manager.stop()
            time.sleep(0.5)
            
            # 禁用步态管理器
            self.gait_manager.disable()
            
            # 执行下台阶动作
            print("Now executing stair_down action...")
            self.motion_manager.run_action('descend_stairs')
            
            # 等待动作完成
            time.sleep(5)
            
            # 恢复准备状态
            self.motion_manager.run_action('walk_ready')
            
            self.descend_executed = True
            self.task_completed = True
            self.current_state = "completed"
            self.active = False
            print("Descend stairs action completed! All stairs navigation finished!")
            self.publish_status("completed")
            common.loginfo('Descend stairs action executed successfully - Task completed')
            
        except Exception as e:
            print(f"Error during descend stairs action: {e}")
            rospy.logerr(f"Descend stairs action error: {e}")
            self.publish_status("error")

    def set_servo(self, pan, tilt):
        """设置舵机位置"""
        try:
            # 使用motion_manager控制舵机，时间200ms，舵机23为左右，舵机24为上下
            self.motion_manager.set_servos_position(200, [[23, pan], [24, tilt]])
        except Exception as e:
            rospy.logerr(f"Servo control error: {e}")

    def shutdown(self, signum, frame):
        """节点关闭回调函数"""
        with self.lock:
            self.motion_manager.run_action('stand')
            self.running = False 
            self.active = False
            self.publish_status("shutdown")
            common.loginfo('%s shutdown' % self.name)

    def run(self):
        """主循环函数"""
        print("Waiting for camera image...")
        
        # 等待图像数据
        while self.current_image is None and self.running:
            rospy.sleep(0.1)
        
        print("Camera ready, stairs task standby...")
        self.publish_status("ready")
        
        # 窗口显示状态
        windows_created = False
        mask_window_created = False
        
        while self.running and not rospy.is_shutdown():
            try:
                # 支持运行时更新显示配置
                self.update_display_config()
                if not self.display_main_window and windows_created:
                    self._safe_destroy_window('Stairs Navigation - Simple Control - ACTIVE')
                    windows_created = False
                if (not self.display_mask_window or not self.display_main_window) and mask_window_created:
                    self._safe_destroy_window('Green Line Mask - ACTIVE')
                    mask_window_created = False

                if self.current_image is not None and self.active:
                    # 检测绿色线条
                    green_line_info, green_mask = self.detect_green_line(self.current_image)
                    
                    # 创建显示图像
                    display_image = self.current_image.copy()
                    
                    # 绘制检测结果
                    if green_line_info is not None:
                        # 绘制检测到的绿色线条
                        x1, y1, x2, y2 = green_line_info['line']
                        cv2.line(display_image, (x1, y1), (x2, y2), (0, 255, 0), 3)
                        
                        # 绘制线条中心点
                        center_x, center_y = green_line_info['center']
                        cv2.circle(display_image, (int(center_x), int(center_y)), 8, (255, 0, 0), -1)
                        
                        # 显示角度信息
                        cv2.putText(display_image, f"Green Angle: {green_line_info['angle']:.1f}°", (10, 30), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                        cv2.putText(display_image, f"State: {self.current_state.upper()}", (10, 60), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 0), 2)
                        cv2.putText(display_image, "STAIRS - SIMPLE CONTROL", (10, 90), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                        
                        # 显示角度状态和位置状态
                        angle_status = "ALIGNED" if abs(green_line_info['angle']) < 5 else f"ANGLE: {green_line_info['angle']:.1f}°"
                        cv2.putText(display_image, angle_status, (10, 120), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0) if abs(green_line_info['angle']) < 5 else (0, 255, 255), 2)
                        
                        # 显示位置状态
                        center_x, center_y = green_line_info['center']
                        action_type = self.get_current_action_type()
                        target_y = self.climb_target_y if action_type == "climb" else self.descend_target_y
                        position_status = f"Y: {center_y:.0f}/{target_y} ({'REACHED' if center_y >= target_y else 'APPROACHING'})"
                        cv2.putText(display_image, position_status, (10, 150), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0) if center_y >= target_y else (255, 255, 0), 2)
                        
                        # 控制机器人移动
                        if not self.task_completed:
                            self.control_robot_movement(green_line_info)
                    else:
                        cv2.putText(display_image, "NO GREEN LINE DETECTED", (10, 30), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
                        cv2.putText(display_image, f"State: {self.current_state.upper()}", (10, 60), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 0), 2)
                        cv2.putText(display_image, "STAIRS - SIMPLE CONTROL", (10, 90), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                        # 在下楼阶段即使没有绿线也继续执行
                        if not self.task_completed:
                            if self.current_state in ["aligning_descend", "descend_ready"]:
                                # 下楼阶段不依赖视觉，直接执行
                                self.control_robot_movement(None)
                            else:
                                # 上楼阶段需要绿线，停止并搜索
                                self.gait_manager.stop()
                                self.publish_status("searching")
                    
                    # 显示任务状态
                    if self.task_completed:
                        cv2.putText(display_image, "TASK COMPLETED", (10, 180), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 255), 2)
                    
                    # 绘制ROI检测区域
                    cv2.rectangle(display_image, (0, self.roi_y_start), (self.image_width, self.roi_y_end), (255, 255, 0), 2)
                    roi_percentage = int(self.roi_y_ratio * 100)
                    cv2.putText(display_image, f"Detection ROI (Bottom {roi_percentage}%)", (10, self.roi_y_start - 10), 
                              cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0), 1)
                    
                    # 绘制目标水平线和中心线
                    action_type = self.get_current_action_type()
                    target_y = self.climb_target_y if action_type == "climb" else self.descend_target_y
                    cv2.line(display_image, (0, target_y), (self.image_width, target_y), (0, 255, 255), 2)
                    cv2.line(display_image, (self.center_x, 0), (self.center_x, self.image_height), (128, 128, 128), 1)
                    target_label = f"{action_type.capitalize()} Target" if action_type else "Target Line"
                    cv2.putText(display_image, target_label, (self.image_width - 150, target_y - 10), 
                              cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1)
                    
                    # 显示图像（只在激活时）
                    if self.display_main_window:
                        cv2.imshow('Stairs Navigation - Simple Control - ACTIVE', display_image)
                        windows_created = True

                        # 显示绿色线条掩码
                        if green_mask is not None and self.display_mask_window:
                            cv2.imshow('Green Line Mask - ACTIVE', green_mask)
                            mask_window_created = True
                        elif mask_window_created and (green_mask is None or not self.display_mask_window):
                            self._safe_destroy_window('Green Line Mask - ACTIVE')
                            mask_window_created = False

                        # 检查退出键
                        key = cv2.waitKey(30) & 0xFF
                        if key == 27:  # ESC键
                            break
                    else:
                        rospy.sleep(0.05)
                        
                elif not self.active and windows_created:
                    # 任务未激活时关闭窗口
                    self._safe_destroy_window('Stairs Navigation - Simple Control - ACTIVE')
                    if mask_window_created:
                        self._safe_destroy_window('Green Line Mask - ACTIVE')
                        mask_window_created = False
                    windows_created = False
                    rospy.sleep(0.1)
                else:
                    rospy.sleep(0.1)
                    
            except KeyboardInterrupt:
                break
            except Exception as e:
                print(f"Error in main loop: {e}")
                rospy.sleep(0.1)
        
        # 退出前动作
        self.gait_manager.stop()
        self.set_servo(self.head_pan_init, self.head_tilt_look_down)
        self.publish_status("stopped")
        if windows_created:
            self._safe_destroy_window('Stairs Navigation - Simple Control - ACTIVE')
        if mask_window_created:
            self._safe_destroy_window('Green Line Mask - ACTIVE')
        rospy.signal_shutdown('shutdown')

if __name__ == '__main__':
    try:
        stairs_tracker = StairsTracker('stairs_tracker')
        stairs_tracker.run()
    except Exception as e:
        print(f"Failed to start stairs tracker: {e}")
        rospy.logerr(f"Stairs tracker initialization error: {e}")
